package com.googlecode.curvedmetromaps.core.logic.forcedriven;

import org.jbox2d.common.Vec2;
import org.jbox2d.dynamics.Body;

import com.googlecode.curvedmetromaps.core.logic.costfunction.CostConstants;
import com.googlecode.curvedmetromaps.core.model.MetroEdge;

/**
 * Constraint S5: minimizes total arc length.
 */
public class ForceS5 extends ForceSubsystem {

	ForceS5(ForceSystem system) {
		super(system);
	}

	void applyForces(int delta) {
		applyEdgeAttractiveForces(delta);
		applyControlPointAttractiveForces(delta);
		applyControlPointRepulsiveForces(delta);
	}

	private void applyEdgeAttractiveForces(int delta) {
		for (MetroEdge edge : system.metroMap.edgeArray) {
			Body m1body = system.stationBodies[edge.src.id];
			Body m2body = system.stationBodies[edge.dst.id];
			Vec2 p1 = m1body.getPosition();
			Vec2 p2 = m2body.getPosition();

			Vec2 direction = p1.sub(p2);
			float len = direction.normalize();
			double f = ForceConstants.ATTRACT1
					* Math.log(len / ForceConstants.ATTRACT2);

			direction.mulLocal((float) f);

			m2body.applyForceToCenter(direction);
			m1body.applyForceToCenter(direction.negateLocal());
		}
	}

	private void applyControlPointAttractiveForces(int delta) {
		for (MetroEdge edge : system.metroMap.edgeArray) {
			Body m1body = system.stationBodies[edge.src.id];
			Body m2body = system.stationBodies[edge.dst.id];
			Body cpbody = system.controlPointBodies[edge.id];
			Vec2 p1 = m1body.getPosition();
			Vec2 p2 = m2body.getPosition();

			Vec2 cp = cpbody.getPosition();

			Vec2 target = p1.add(p2);
			target.mulLocal(0.5f);

			Vec2 direction = target.sub(cp);
			float len = direction.normalize();
			len = (len < 0.1f) ? 0.1f : len;

			if (len > 0.0f) {
				double f = ForceConstants.ATTRACT3
						* Math.log(len / ForceConstants.ATTRACT4);

				direction.mulLocal((float) f);

				cpbody.applyForceToCenter(direction);
			}
		}
	}

	private void applyControlPointRepulsiveForces(int delta) {
		for (MetroEdge edge : system.metroMap.edgeArray) {
			Body m1body = system.stationBodies[edge.src.id];
			Body m2body = system.stationBodies[edge.dst.id];
			Body cpbody = system.controlPointBodies[edge.id];
			Vec2 cp = cpbody.getPosition();

			Body[] bodies = { m1body, m2body };
			for (Body b : bodies) {
				Vec2 p = b.getPosition();
				Vec2 direction = cp.sub(p);
				float len = direction.normalize();

				double f = ForceConstants.S5_REPULSE_CP * Math.exp(-CostConstants.S5_LAMBDA_CP * len / CostConstants.S5_DVMIN_CP);
				
				direction.mulLocal((float) f);

				cpbody.applyForceToCenter(direction);
			}
		}
	}
}
